#pragma once
#include"Particle.h"
#include <deque>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Eigen>
#include <tuple>
using namespace std;
class ParticlesData
{

public:
	using Particles = std::deque<Particle<Pose3D>>;
	Particles m_particles;
	double max_weight = 0.0;
	double min_weight = 0.0;

	/*double max_x{0.0};
	double min_x{ 0.0 };
	double max_y{ 0.0 };
	double min_y{ 0.0 };
	double max_z{ 0.0 };
	double min_z{ 0.0 };*/



	double init_x{ 0.0 };
	double init_y{ 0.0 };
	double init_z{ 0.0 };
	double init_yaw{ 0.0 };

	ParticlesData();
	~ParticlesData();
	void DisplayParticleWeight();
	double GetSumOfParticle();
	double getMaxWeight();
	Particle<Pose3D>* getMostLikelyParticle();
	std::tuple<Eigen::MatrixXd, Pose3D>getCovarianceAndMean();
	Pose3D getMean(Pose3D& pose);
	void append(const Pose3D& p, const double weight);
	void append(const Eigen::Matrix3d& rot, const double weight);
	void getAverage(Pose3D& return_mean);
	Pose3D getLastPose(size_t index);
	void updateParticleWithNewPose(Pose3D& particlePose, Pose3D  newPose);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getParticlesAsPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getParticlesAsPointCloud(const Particles& shift_particles, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
	void logOutPutAboutParticles(string root);
	void saveGuassCovarianceAndMean();
	void saveResampleCovarianceAndMean();
	void saveShiftCovarianceAndMean();
	void saveSingleIterateOfAllParticlesPF();
	void saveSingleIterateOfAllParticlesSPF();

private:
	double m_count{ 0 };
	double m_accum_x{ 0 }, m_accum_y{ 0 }, m_accum_z{ 0 };
	double m_accum_yaw{ 0 }, m_accum_pitch{ 0 }, m_accum_roll{ 0 };
	Eigen::Matrix3d m_rot_part;

	vector<Pose3D> guassPoseMean;
	vector<Pose3D> resamplePoseMean;
	vector<Pose3D> shiftPoseMean;




	vector<Eigen::MatrixXd> guassPoseCov;
	vector<Eigen::MatrixXd> resamplePoseCov;
	vector<Eigen::MatrixXd> shiftPoseCov;

	vector<Pose3D> singleIterateParticlesPF;
	vector<Pose3D> singleIterateParticlesSPF;

};
